// Do not remove the include below
#include "arduino-serial-canbus-gateway.h"

// read serial message and load toCanQueue
void readSerialTask() {
	byte ret;
	ret = 0;
	while ((Serial.available() > 0) && (ret == 0))
		ret = HDLCProtocol.processData(Serial.read());
}
// Read message from CANBus
void readCanTask() {
	Can::CAN_MSG canMsg;
	if (Can.readMessage(&canMsg))
		fromCanQueue.push(canMsg);
}
// send CAN message to Serial
void readFromCanQueueTask() {
	if (fromCanQueue.count() > 0)
		HDLCProtocol.startXMit();
}
// read toCanQueue for sending message to CAN
void readToCanQueueTask() {
	Can::CAN_MSG canMsg;
	if (toCanQueue.count() > 0) {
		canMsg = toCanQueue.peek();
		if (Can.sendMessage(&canMsg))
			toCanQueue.pop();
	}
}

//The setup function is called once at startup of the sketch
void setup()
{
	// Initialize SPI protocol whith MCP2515
	Spi.init();
	// Gateway initialize Serial port
	Serial.begin(115200);
	// initialise CAN MCP2515
	Can.init();
	Can.setMode(Can::_OPERATION_);
}

// The loop function is called in an endless loop
void loop()
{
	readSerialTask();
	readToCanQueueTask();
	readFromCanQueueTask();
	readCanTask();
}
